{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:33:38.441778Z",
     "iopub.status.busy": "2023-08-08T05:33:38.441192Z",
     "iopub.status.idle": "2023-08-08T05:33:44.957660Z",
     "shell.execute_reply": "2023-08-08T05:33:44.956089Z",
     "shell.execute_reply.started": "2023-08-08T05:33:38.441736Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Looking in indexes: https://pypi.tuna.tsinghua.edu.cn/simple\r\n",
      "Collecting opencv-python==4.5.2.52\r\n",
      "  Downloading https://pypi.tuna.tsinghua.edu.cn/packages/35/7c/353c4e264a688a292ac4886cca5747ad8858452cede431f7fcd6fb26abe7/opencv_python-4.5.2.52-cp37-cp37m-manylinux2014_x86_64.whl (51.0 MB)\r\n",
      "\u001b[2K     \u001b[90m━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━\u001b[0m \u001b[32m51.0/51.0 MB\u001b[0m \u001b[31m14.3 MB/s\u001b[0m eta \u001b[36m0:00:00\u001b[0m00:01\u001b[0m00:01\u001b[0m\r\n",
      "\u001b[?25hRequirement already satisfied: numpy>=1.14.5 in /opt/conda/envs/python35-paddle120-env/lib/python3.7/site-packages (from opencv-python==4.5.2.52) (1.19.5)\r\n",
      "Installing collected packages: opencv-python\r\n",
      "  Attempting uninstall: opencv-python\r\n",
      "    Found existing installation: opencv-python 4.1.1.26\r\n",
      "    Uninstalling opencv-python-4.1.1.26:\r\n",
      "      Successfully uninstalled opencv-python-4.1.1.26\r\n",
      "Successfully installed opencv-python-4.5.2.52\r\n",
      "\r\n",
      "\u001b[1m[\u001b[0m\u001b[34;49mnotice\u001b[0m\u001b[1;39;49m]\u001b[0m\u001b[39;49m A new release of pip available: \u001b[0m\u001b[31;49m22.1.2\u001b[0m\u001b[39;49m -> \u001b[0m\u001b[32;49m23.2.1\u001b[0m\r\n",
      "\u001b[1m[\u001b[0m\u001b[34;49mnotice\u001b[0m\u001b[1;39;49m]\u001b[0m\u001b[39;49m To update, run: \u001b[0m\u001b[32;49mpip install --upgrade pip\u001b[0m\r\n"
     ]
    }
   ],
   "source": [
    "!pip install --upgrade opencv-python==4.5.2.52"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:33:44.972746Z",
     "iopub.status.busy": "2023-08-08T05:33:44.972364Z",
     "iopub.status.idle": "2023-08-08T05:33:46.073672Z",
     "shell.execute_reply": "2023-08-08T05:33:46.072658Z",
     "shell.execute_reply.started": "2023-08-08T05:33:44.972718Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "import os\n",
    "import numpy as np\n",
    "import cv2\n",
    "import csv\n",
    "from glob import glob\n",
    "import matplotlib.pyplot as plt\n",
    "from collections import namedtuple\n",
    "from copy import deepcopy\n",
    "from tqdm import tqdm\n",
    "import random\n",
    "random.seed(1314)\n",
    "# 确认opencv的版本，如果报错，重启内核再运行本cell即可\n",
    "assert cv2.__version__ > '4.5', 'Please use OpenCV 4.5 or later.'"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "# 1) 数据集介绍\n",
    "\n",
    "- 数据集是同一场景下的图像对。每一对图像都对应了一组缩放因子。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T04:53:13.820003Z",
     "iopub.status.busy": "2023-08-08T04:53:13.819442Z",
     "iopub.status.idle": "2023-08-08T04:53:28.280496Z",
     "shell.execute_reply": "2023-08-08T04:53:28.279348Z",
     "shell.execute_reply.started": "2023-08-08T04:53:13.819971Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "# 解压数据集\n",
    "\n",
    "!unzip -qo data/data230502/大视角差图像特征提取及匹配挑战赛公开数据-初赛.zip -d ./work/image-matching-challenge-iflytek\n",
    "!mv work/image-matching-challenge-iflytek/┤є╩╙╜╟▓ю═╝╧ё╠╪╒ў╠с╚б╝░╞е┼ф╠Ї╒╜╚№╣л┐к╩¤╛▌-│ї╚№/* work/image-matching-challenge-iflytek/\n",
    "!rm -rf work/image-matching-challenge-iflytek/┤є╩╙╜╟▓ю═╝╧ё╠╪╒ў╠с╚б╝░╞е┼ф╠Ї╒╜╚№╣л┐к╩¤╛▌-│ї╚№  "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "# 2) 数据集处理分析\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "- 关键性处理函数的定义"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:34:18.936454Z",
     "iopub.status.busy": "2023-08-08T05:34:18.935854Z",
     "iopub.status.idle": "2023-08-08T05:34:18.946323Z",
     "shell.execute_reply": "2023-08-08T05:34:18.945194Z",
     "shell.execute_reply.started": "2023-08-08T05:34:18.936414Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "# 关键函数的定义\n",
    "\n",
    "# 一个元组，包含相机的内参矩阵K，以及相机的外参R和T\n",
    "Gt = namedtuple('Gt', ['K', 'R', 'T'])\n",
    "\n",
    "# 一个小的ε\n",
    "eps = 1e-15\n",
    "\n",
    "src = './work/image-matching-challenge-iflytek'\n",
    "\n",
    "def NormalizeKeypoints(keypoints, K):\n",
    "    C_x = K[0, 2]\n",
    "    C_y = K[1, 2]\n",
    "    f_x = K[0, 0]\n",
    "    f_y = K[1, 1]\n",
    "    keypoints = (keypoints - np.array([[C_x, C_y]])) / np.array([[f_x, f_y]])\n",
    "    return keypoints\n",
    "\n",
    "def ComputeMaa(err_q, err_t, thresholds_q, thresholds_t):\n",
    "    '''通过不同的阈值，计算一个场景的Maa 平均的平均准确度.'''\n",
    "    \n",
    "    assert len(err_q) == len(err_t)\n",
    "    \n",
    "    acc, acc_q, acc_t = [], [], []\n",
    "    for th_q, th_t in zip(thresholds_q, thresholds_t):\n",
    "        acc += [(np.bitwise_and(np.array(err_q) < th_q, np.array(err_t) < th_t)).sum() / len(err_q)]\n",
    "        acc_q += [(np.array(err_q) < th_q).sum() / len(err_q)]\n",
    "        acc_t += [(np.array(err_t) < th_t).sum() / len(err_t)]\n",
    "    return np.mean(acc), np.array(acc), np.array(acc_q), np.array(acc_t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:34:22.621302Z",
     "iopub.status.busy": "2023-08-08T05:34:22.619667Z",
     "iopub.status.idle": "2023-08-08T05:34:22.630044Z",
     "shell.execute_reply": "2023-08-08T05:34:22.628864Z",
     "shell.execute_reply.started": "2023-08-08T05:34:22.621229Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "def ReadpairsData(filename):\n",
    "    # 1) define dict \n",
    "    covisibility_dict = {}\n",
    "    # 2) \n",
    "        # 2.1 open csv file\n",
    "    with open(filename) as f:\n",
    "        # 2.2 read csv file\n",
    "        reader = csv.reader(f, delimiter=',')\n",
    "        # 2.3 load data\n",
    "        for i, row in enumerate(reader):\n",
    "            # Skip header.\n",
    "            if i == 0:\n",
    "                continue\n",
    "            covisibility_dict[row[0]] = 0\n",
    "    # 3) return \n",
    "    return covisibility_dict"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:34:23.773378Z",
     "iopub.status.busy": "2023-08-08T05:34:23.771344Z",
     "iopub.status.idle": "2023-08-08T05:34:23.781685Z",
     "shell.execute_reply": "2023-08-08T05:34:23.780792Z",
     "shell.execute_reply.started": "2023-08-08T05:34:23.773297Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "def LoadCalibration_test(filename):\n",
    "    '''从.csv文件中加载真值'''\n",
    "    # 1) 创建变量用于输出\n",
    "    calib_dict = {}\n",
    "    # 2) 处理文件\n",
    "        # 2.1 打开文件\n",
    "    with open(filename, 'r') as f:\n",
    "        # 2.2 读取数据\n",
    "        reader = csv.reader(f, delimiter=',')\n",
    "        # 2.3 遍历数据\n",
    "        for i, row in enumerate(reader):\n",
    "            # 2.3.1 Skip header.\n",
    "            if i == 0:\n",
    "                continue\n",
    "            # 2.3.2 提取数据\n",
    "                # 2.3.2.1 图像编号\n",
    "            camera_id = row[0]\n",
    "                # 2.3.2.2 相机内参\n",
    "            K = np.array([float(v) for v in row[1].split(' ')]).reshape([3, 3])\n",
    "                # 2.3.2.3 旋转矩阵\n",
    "            R = np.zeros((3,3))\n",
    "                # 2.3.2.4 位移向量\n",
    "            T = np.zeros((3,3))\n",
    "            # 2.3.3 打包变量输出\n",
    "            calib_dict[camera_id] = Gt(K=K, R=R, T=T)\n",
    "    \n",
    "    return calib_dict "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:48:40.015059Z",
     "iopub.status.busy": "2023-08-08T05:48:40.013871Z",
     "iopub.status.idle": "2023-08-08T05:48:40.027716Z",
     "shell.execute_reply": "2023-08-08T05:48:40.026730Z",
     "shell.execute_reply.started": "2023-08-08T05:48:40.015006Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Loaded 202 images.\r\n"
     ]
    }
   ],
   "source": [
    "pairs = ReadpairsData(f'{src}/test/pair.csv')    \n",
    "\n",
    "calib_dict = LoadCalibration_test(f'{src}/test/calibration.csv')\n",
    "\n",
    "images_path_dict={}\n",
    "for filename in glob(f'{src}/test/images/*'):\n",
    "    cur_id = os.path.basename(os.path.splitext(filename)[0])\n",
    "    images_path_dict[cur_id] = filename\n",
    "print(f'Loaded {len(images_path_dict)} images.')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "# 3) SIFT算法实验\n",
    "\n",
    "- 该比赛的目标是要得到两张照片的相对姿态，即旋转矩阵R与平移向量T\n",
    "- 本项目先用比较经典的**sift算法**提取特征点并进行匹配、计算像片对的相对位姿"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:35:19.911039Z",
     "iopub.status.busy": "2023-08-08T05:35:19.909770Z",
     "iopub.status.idle": "2023-08-08T05:35:19.917096Z",
     "shell.execute_reply": "2023-08-08T05:35:19.915730Z",
     "shell.execute_reply.started": "2023-08-08T05:35:19.910991Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "# input:\n",
    "    # image: 图片\n",
    "    # detector: 检测器\n",
    "    # num_features: 特征点数量\n",
    "# output:\n",
    "    # kp[:num_features]: 关键点\n",
    "    # desc[:num_features]: 描述子\n",
    "def ExtractSiftFeatures(image, detector, num_features):\n",
    "    '''计算图像的SIFT特征'''\n",
    "    gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)\n",
    "    kp, desc = detector.detectAndCompute(gray, None)\n",
    "    return kp[:num_features], desc[:num_features]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:35:22.596484Z",
     "iopub.status.busy": "2023-08-08T05:35:22.595580Z",
     "iopub.status.idle": "2023-08-08T05:35:22.602361Z",
     "shell.execute_reply": "2023-08-08T05:35:22.601211Z",
     "shell.execute_reply.started": "2023-08-08T05:35:22.596432Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "def ArrayFromCvKps(kps):\n",
    "    '''将opencv返回的关键点转为numpy矩阵的形式'''\n",
    "    \n",
    "    return np.array([kp.pt for kp in kps])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:36:01.256009Z",
     "iopub.status.busy": "2023-08-08T05:36:01.255280Z",
     "iopub.status.idle": "2023-08-08T05:36:01.263082Z",
     "shell.execute_reply": "2023-08-08T05:36:01.262113Z",
     "shell.execute_reply.started": "2023-08-08T05:36:01.255958Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "def ComputeEssentialMatrix(F, K1, K2, kp1, kp2):\n",
    "    ''''在给定校准矩阵的情况下，从基础矩阵计算本质矩阵，参赛者需要不依赖给定的内参计算基础矩阵'''\n",
    "    \n",
    "    # 老版本的OpenCV计算基础矩阵是返回多个，本项目使用版本为：   \n",
    "    # https://opencv.org/evaluating-opencvs-new-ransacs\n",
    "    assert F.shape[0] == 3, 'Malformed F?'\n",
    "\n",
    "    # 使用 OpenCV的 recoverPose 功能计算两张照片的相对位姿R,t:\n",
    "    # https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#gadb7d2dfcc184c1d2f496d8639f4371c0\n",
    "    E = np.matmul(np.matmul(K2.T, F), K1).astype(np.float64)\n",
    "    \n",
    "    kp1n = NormalizeKeypoints(kp1, K1)\n",
    "    kp2n = NormalizeKeypoints(kp2, K2)\n",
    "    num_inliers, R, T, mask = cv2.recoverPose(E, kp1n, kp2n)\n",
    "\n",
    "    return E, R, T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:36:24.106277Z",
     "iopub.status.busy": "2023-08-08T05:36:24.105022Z",
     "iopub.status.idle": "2023-08-08T05:36:24.111560Z",
     "shell.execute_reply": "2023-08-08T05:36:24.110605Z",
     "shell.execute_reply.started": "2023-08-08T05:36:24.106223Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "show_images = False\n",
    "num_show_images = 1\n",
    "max_pairs_per_scene = 50\n",
    "verbose = False\n",
    "\n",
    "# We use two different sets of thresholds over rotation and translation. Do not change this -- these are the values used by the scoring back-end.\n",
    "thresholds_q = np.linspace(1, 10, 10)\n",
    "thresholds_t = np.geomspace(0.2, 5, 10)\n",
    "\n",
    "# Instantiate the matcher.\n",
    "bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True)\n",
    "num_features=5000\n",
    "sift_detector = cv2.SIFT_create(num_features, contrastThreshold=-10000, edgeThreshold=-10000)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:38:00.660342Z",
     "iopub.status.busy": "2023-08-08T05:38:00.659672Z",
     "iopub.status.idle": "2023-08-08T05:44:42.986497Z",
     "shell.execute_reply": "2023-08-08T05:44:42.985140Z",
     "shell.execute_reply.started": "2023-08-08T05:38:00.660292Z"
    },
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "images_dict = {}\n",
    "kp_dict = {}\n",
    "desc_dict = {}\n",
    "# print('Extracting features...')\n",
    "for id in images_path_dict:\n",
    "    images_dict[id] = cv2.cvtColor(cv2.imread(images_path_dict[id]), cv2.COLOR_BGR2RGB)\n",
    "    kp_dict[id], desc_dict[id] = ExtractSiftFeatures(images_dict[id], sift_detector, 2000)\n",
    "# print()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "F_dict={}\n",
    "for counter, pair in enumerate(pairs):\n",
    "    print('---read---')\n",
    "    id1, id2 = pair.split('-')\n",
    "    #------------------------\n",
    "    cv_matches = bf.match(desc_dict[id1], desc_dict[id2])\n",
    "    matches = np.array([[m.queryIdx, m.trainIdx] for m in cv_matches])\n",
    "    cur_kp_1 = ArrayFromCvKps([kp_dict[id1][m[0]] for m in matches])\n",
    "    cur_kp_2 = ArrayFromCvKps([kp_dict[id2][m[1]] for m in matches])\n",
    "\n",
    "    # Filter matches with RANSAC.\n",
    "    F, inlier_mask = cv2.findFundamentalMat(cur_kp_1, cur_kp_2, cv2.USAC_MAGSAC, 0.25, 0.99999, 10000)\n",
    "    inlier_mask = inlier_mask.astype(bool).flatten()\n",
    "    \n",
    "    matches_after_ransac = np.array([match for match, is_inlier in zip(matches, inlier_mask) if is_inlier])\n",
    "    inlier_kp_1 = ArrayFromCvKps([kp_dict[id1][m[0]] for m in matches_after_ransac])\n",
    "    inlier_kp_2 = ArrayFromCvKps([kp_dict[id2][m[1]] for m in matches_after_ransac])\n",
    "\n",
    "    # Compute the essential matrix.\n",
    "    print('--Essential--')\n",
    "    E, R, T = ComputeEssentialMatrix(F, calib_dict[id1].K, calib_dict[id2].K, inlier_kp_1, inlier_kp_2)\n",
    "    F_dict[pair]=[R,T]\n",
    "  "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 4) Submit"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:45:12.973170Z",
     "iopub.status.busy": "2023-08-08T05:45:12.972456Z",
     "iopub.status.idle": "2023-08-08T05:45:12.978509Z",
     "shell.execute_reply": "2023-08-08T05:45:12.977526Z",
     "shell.execute_reply.started": "2023-08-08T05:45:12.973118Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "def FlattenMatrix(M, num_digits=8):\n",
    "    '''Convenience function to write CSV files.'''\n",
    "    return ' '.join([f'{v:.{num_digits}e}' for v in M.flatten()])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true,
    "tags": []
   },
   "outputs": [],
   "source": [
    "for sample_id, F in F_dict.items():\n",
    "        print(sample_id)\n",
    "        print(FlattenMatrix(F[0]))\n",
    "        print(FlattenMatrix(F[1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-08T05:45:17.124779Z",
     "iopub.status.busy": "2023-08-08T05:45:17.124105Z",
     "iopub.status.idle": "2023-08-08T05:45:17.134556Z",
     "shell.execute_reply": "2023-08-08T05:45:17.133651Z",
     "shell.execute_reply.started": "2023-08-08T05:45:17.124740Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "with open('submission.csv', 'w') as f:\n",
    "    f.write('pair,rotation_matrix,translation_vector\\n')\n",
    "    for sample_id, F in F_dict.items():\n",
    "        f.write(f'{sample_id},{FlattenMatrix(F[0])},{FlattenMatrix(F[1])}\\n')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "# 5) Ref and conclusion\n",
    "\n",
    "\n",
    "[[1] 图像匹配挑战赛介绍，并以SuperGlue与SIFT特征点匹配实验对比精度](https://aistudio.baidu.com/aistudio/projectdetail/4196840?channelType=0&channel=0)\n",
    "\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "py35-paddle1.2.0"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
